#include "ros/ros.h"
#include "std_msgs/String.h" 
#include <sstream>
#include <iostream>

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc/imgproc_c.h"
#include <opencv2/calib3d.hpp>

#include<stdio.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include "Params.h"
#include "MatchLights.hpp"

using namespace std;
using namespace cv;

Mat img;
void doMsg(const sensor_msgs::ImageConstPtr& msg){

    img=cv_bridge::toCvShare(msg,"bgr8")->image;
    Params param;
    //图像预处理
    vector<Mat> channels;
    Mat grayimg,binBrightImg;
    //取蓝色通道    
    split(img,channels);
    grayimg=(channels.at(0));
    //二值
    threshold(grayimg, binBrightImg, param.deal_threshold, 255, cv::THRESH_BINARY);
    //膨胀
    Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3));
    dilate(binBrightImg, binBrightImg, element);

    //找轮廓
    vector<vector<Point> > Contours;
    vector<LightDescriptor> lights;
    findContours(binBrightImg.clone(), Contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    //挑选轮廓
    float contArea;RotatedRect rect;
    for(int i=0;i<Contours.size();i++){

        contArea=contourArea(Contours[i]);
        //面积约束
        if(contArea<param.choose_min_area || contArea>param.choose_max_area){continue;}
        
        rect=fitEllipse(Contours[i]);
        //长宽比例约束
        if(rect.size.height/rect.size.width<param.choose_ratio){continue;}
        //倾斜角度约束
        if(rect.angle>param.choose_floor_angle && rect.angle<param.choose_ceil_angle){continue;}
        lights.push_back(LightDescriptor(rect));
    }

    //进行匹配
    vector<MatchLights> matc;
    for(int i=0;i<lights.size()-1;i++){
        for(int j=i+1;j<lights.size();j++){
            if(abs(lights[i].angle-lights[j].angle)>param.match_dangle){continue;}
            if(abs(lights[i].center.x-lights[j].center.x)>param.match_max_dx){continue;}
            if(abs(lights[i].center.y-lights[j].center.y)>param.match_dy){continue;}
            if(abs(lights[i].length-lights[j].length)>param.match_dlength){continue;}
            matc.push_back(MatchLights(lights[i],lights[j]));
        }
    }

    //对匹配结果检查
    int i=0;
    while(i<matc.size()-1 && matc.size()!=0){
        int j=i+1;
        while(j<matc.size()){
            if(abs(matc[i].center.x-matc[j].center.x)<param.check_distance){
                if(matc[i].score>matc[j].score)
                {matc.erase(matc.begin()+i);}
                else{matc.erase(matc.begin()+j);}
                continue;
            }
            j++;
        }
        i++;
    }

    //PnP解算
    Vector3f xyz;
    ostringstream oss1,oss2,oss3;
    if(matc.size()!=0)
    {
        xyz=matc[0].PNP();
        oss1<<"x="<<xyz[0]<<"mm";
        oss2<<"y="<<xyz[1]<<"mm";
        oss3<<"z="<<xyz[2]<<"mm";
        //cout<<xyz[0]<<endl;
    }else{oss1<<"x=";oss2<<"y=";oss3<<"z=";}
    string str1(oss1.str());
    string str2(oss2.str());
    string str3(oss3.str());
    putText(img,str1,Point2i(20,50),cv::FONT_HERSHEY_COMPLEX,1,Scalar(0,255,0),2);
    putText(img,str2,Point2i(20,90),cv::FONT_HERSHEY_COMPLEX,1,Scalar(0,255,0),2);
    putText(img,str3,Point2i(20,130),cv::FONT_HERSHEY_COMPLEX,1,Scalar(0,255,0),2);

    //显示画面
    for(int i=0;i<matc.size();i++){   
        matc[i].draw(img);
    }

    /*for(int i=0;i<lights.size();i++){
        lights[i].draw(img);
    }*/

    imshow("view",img);
    waitKey(1);
    
    
}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"listenerb");
    ros::NodeHandle nh;
    cv::namedWindow("view");
    cv::startWindowThread();
    image_transport::ImageTransport transport(nh);

    image_transport::Subscriber sub;
    sub=transport.subscribe("OutImage",1,doMsg);
    ros::spin();
    destroyWindow("view");

    return 0;
}
